#include "PhysicsBody.h"
#include "FragmentationDemo.h"

#include <btBulletDynamicsCommon.h>
#include <assert.h>

PhysicsBody::PhysicsBody() : m_pRigidBody(0), m_pMotionState(0), m_localInertia(0,0,0), m_mass(0), m_isDynamic(false)
{


}

bool PhysicsBody::CreateRigidBody(btCollisionShape* shape, float mass) 
{ 
	//mp_collShape = shape;

	m_mass = mass;
	
	if (m_mass != 0.0f)
	{
		m_isDynamic = true;
		shape->calculateLocalInertia(m_mass, m_localInertia);
	}
	


	btTransform trans;
	trans.setIdentity();
	trans.setOrigin(btVector3(m_pos.X, m_pos.Y, m_pos.Z));

	m_pMotionState = new btDefaultMotionState(trans);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(m_mass, m_pMotionState, shape, m_localInertia);
	m_pRigidBody = new btRigidBody(rbInfo);
	assert(m_pRigidBody);

	TheFragmentationDemo::Instance()->AddCollisionShape(shape);

	m_pRigidBody->setUserPointer(this);

	TheFragmentationDemo::Instance()->GetDynamicsWorld()->addRigidBody(m_pRigidBody);


	return true;

}

inline void PhysicsBody::SetPos(const Vec3f& pos)
{ 
	btTransform trans;
	trans.setIdentity();
	trans.setOrigin(btVector3(pos.X, pos.Y, pos.Z));
	m_pRigidBody->setWorldTransform(trans);
}

inline Vec3f PhysicsBody::GetPos() const
{
	return m_pos;
}

inline void PhysicsBody::SetVel(const Vec3f& vel)
{
	btVector3 newVel(vel.X, vel.Y, vel.Z);
	m_pRigidBody->setLinearVelocity(newVel);
}

inline Vec3f PhysicsBody::GetVel() const
{
	btVector3 vel = m_pRigidBody->getLinearVelocity();
	Vec3f result(vel.x(), vel.y(), vel.z()); 
	return result;
}

btRigidBody* PhysicsBody::GetRigidBody()
{
	return m_pRigidBody;
}
